/**
 * @file SawtoothWallPlanner.h
 * @author 
 * @brief 
 * @version 1.0
 * @date 2020-08-20
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "lib/contour.h"

namespace behavior_controller
{

    class SawtoothWallPlanner
    {
    public:
        /**
         * @brief 阶跃的墙面检测构造函数
         * @return
         */
        SawtoothWallPlanner(){};

        /**
         * @brief 阶跃的墙面检测运行函数
         * @return
         */
        void run(double &linear_vel, double &angular_vel);

        /**
         * @brief 机器人检测到Z形墙面后,超越墙面向前走半个机身。再转向90度，朝向墙面行进。
         * @return
         */
        void exceed_wall_front_move(double &linear_vel, double &angular_vel);

        /**
         * @brief 转向90度，朝向墙面行进。
         * @return
         */
        void rotation_right_angle(double &linear_vel, double &angular_vel);

        /**
         * @brief 朝向墙面行进。
         * @return
         */
        void front_move(double &linear_vel, double &angular_vel);

        bool finished = true; //结束标志，需要撞到Bumper、或超过1秒后结束
        double time_now = 0;   //记录单个状态中的开始时间
        int state_step = 0;   //记录当前流程状态
    };

    //static 
    extern SawtoothWallPlanner sawtooth_wall_planner;
} // namespace behavior_controller